#include "Game.h"
#include "Screen.h"
#include "Timer.h"
#include "EventPoller.h"
#include "GUIManager.h"
#include <assert.h>

#include "btBulletDynamicsCommon.h"

Game::Game() : mp_State(nullptr)
{
	m_frameNumber =0;
	mp_collisionConfiguration = (nullptr);
	mp_dispatcher = (nullptr);
	mp_broadphase = (nullptr);
	mp_constraintSolver = (nullptr);
	mp_dynamicsWorld = (nullptr);
	mp_collisionShapes = (new btAlignedObjectArray<btCollisionShape*>);
	m_collisionOn = true;
	m_gravity = Vec3f(0,-20,0);

}

void Game::Start()
{
	while(true)
	{
		m_frameNumber++;
		assert(m_frameNumber >= 0);

		Update();
		Draw();
		TheScreen::Instance()->Flip();
		
	}
}

void Game::Draw()
{
	mp_State->Draw(); 
	
	//DrawGameObjects();

	if(m_collisionOn)
	{
		//mp_dynamicsWorld->debugDrawWorld();
	}
	
}

GameObject* Game::GetGameObject(int id)
{
	return m_objects.find(id)->second;
}

void Game::Update()
{
	TheTimer::Instance()->Update();
	TheEventPoller::Instance()->Update();
	//UpdateGameObjects();
	
	mp_State->Update();
	if(m_collisionOn)
	{
		
		mp_dynamicsWorld->stepSimulation(TheTimer::Instance()->GetDT() * 2); 
		BTCheckPairs();
	}
	else 
	{
		mp_dynamicsWorld->stepSimulation(0.0f);
	}
}


void Game::SetGameState(GameState* theGameState)
{
	if (mp_State != theGameState)
	{
		if (mp_State)
		{
			mp_State->OnDeactivated();
		}

		mp_State = theGameState;
		mp_State->OnActivated();
	}
}

void Game::DrawGameObjects()
{
	m_it = m_objects.begin();
	for ( ; m_it != m_objects.end(); ++m_it)
	{
		m_it->second->Draw();
	}

}

void Game::UpdateGameObjects()
{
	m_it = m_objects.begin();
	for ( ;	m_it != m_objects.end(); ++m_it)
	{
		m_it->second->Update();
	}
}

void Game::AddGameObject(int id, GameObject* g)
{
	assert(m_objects.find(id) == m_objects.end());
	m_objects[id] = g;
}

void Game::BTCollisionsOn(bool coll)
{
	m_collisionOn = coll;
}

void Game::BTCheckPairs()
{
	//Assume world->stepSimulation or world->performDiscreteCollisionDetection has been called

	unsigned int i, j;
	//m_numManifolds = mp_broadphase->getOverlappingPairCache()->getNumOverlappingPairs();
	//i = mp_dynamicsWorld->getPairCache()->getNumOverlappingPairs();
	m_numManifolds = TheGame::Instance()->GetDynWorld()->getDispatcher()->getNumManifolds();

	for ( i = 0; i < m_numManifolds; i++)
	{
		mp_manifold =  TheGame::Instance()->GetDynWorld()->getDispatcher()->getManifoldByIndexInternal(i);
		//TheGame::Instance()->GetDynWorld()->getDispatcher()->Ge
		mp_obA = static_cast<btCollisionObject*>(mp_manifold->getBody0());
		mp_obB = static_cast<btCollisionObject*>(mp_manifold->getBody1());
	
		m_numContacts = mp_manifold->getNumContacts();
		for ( j=0; j < m_numContacts; j++)
		{
			btManifoldPoint& pt = mp_manifold->getContactPoint(j);
			if (pt.getDistance()<0.f)
			{
				btVector3 aAng = mp_obA->getInterpolationAngularVelocity();
				btVector3 aLin = mp_obA->getInterpolationLinearVelocity();
				btVector3 bAng = mp_obA->getInterpolationAngularVelocity();
				btVector3 bLin = mp_obA->getInterpolationLinearVelocity();

				GameObject* go1 = static_cast<GameObject*>(mp_obA->getUserPointer());
				GameObject* go2 = static_cast<GameObject*>(mp_obB->getUserPointer());
				go1->BTCollided(go2, aAng, aLin);
				go2->BTCollided(go1, bAng, bLin);
			}
		}
	}
}


void Game::BTReset()
{
	mp_collisionShapes->resize(0);
	delete mp_collisionConfiguration;
	delete mp_dispatcher; 
	delete mp_broadphase;
	delete mp_constraintSolver;
	delete mp_dynamicsWorld;
	delete mp_manifold;

	btDefaultCollisionConfiguration* collisionConfig = new btDefaultCollisionConfiguration;
	BTSetDefaultCollisionConfig(collisionConfig);
	BTSetCollisionDispatcher(new btCollisionDispatcher(collisionConfig));
	BTSetBroadPhase(new btDbvtBroadphase);
	BTSetConstraintSolver(new btSequentialImpulseConstraintSolver);
	BTCreateDynamicsWorld();
	
}
btBroadphaseInterface* Game::GetBroadInterface()
{
	return mp_broadphase;
}

///collision configuration contains default setup for memory, collision setup. Advanced users can create their own configuration.
void Game::BTSetDefaultCollisionConfig(btDefaultCollisionConfiguration* config)
{
	mp_collisionConfiguration = config; 
}

//the default collision dispatcher.
void Game::BTSetCollisionDispatcher(btCollisionDispatcher* dispatch)
{
	mp_dispatcher = dispatch;
}

///btDbvtBroadphase is a good general purpose broadphase. btAxis3Sweep for sweep and prune.
void Game::BTSetBroadPhase(btBroadphaseInterface* broad)
{
	mp_broadphase = broad;
}

//the default constraint solver. 
void Game::BTSetConstraintSolver(btSequentialImpulseConstraintSolver* solver)
{
	mp_constraintSolver = solver;
}

//void Game::BTDebugDrawSet(btIDebugDraw* deb)
//{
//	mp_debugDraw = deb;
//}

btDiscreteDynamicsWorld* Game::GetDynWorld()
{
	return mp_dynamicsWorld;
}

bool Game::BTCreateDynamicsWorld()
{
	if(mp_dispatcher && mp_broadphase && mp_constraintSolver && mp_collisionConfiguration)
	{
		mp_dynamicsWorld = new btDiscreteDynamicsWorld(mp_dispatcher, mp_broadphase, mp_constraintSolver, mp_collisionConfiguration);
		mp_dynamicsWorld->setGravity(btVector3(m_gravity.X, m_gravity.Y, m_gravity.Z));
		return true;
	}
	return false;
}

void Game::DeleteGameObjects()
{
	m_it = m_objects.begin();
	for ( ; m_it != m_objects.end(); ++m_it)
	{
		delete m_it->second;
	}
	m_objects.clear();
}

int Game::GetFrameNumber()
{
	return m_frameNumber;
}

Game::~Game()
{
	//cleanup in the reverse order of creation/initialization

	//remove the rigidbodies from the dynamics world and delete them
	for (int i = mp_dynamicsWorld->getNumCollisionObjects()-1; i>=0 ;i--)
	{
		btCollisionObject* obj = mp_dynamicsWorld->getCollisionObjectArray()[i];
		btRigidBody* body = btRigidBody::upcast(obj);
		if (body && body->getMotionState())
		{
			delete body->getMotionState();
		}
		mp_dynamicsWorld->removeCollisionObject( obj );
		delete obj;
	}

	//delete collision shapes
	for (int j=0;j< mp_collisionShapes->size();j++)
	{
		btCollisionShape* shape = (*mp_collisionShapes)[j];
		(*mp_collisionShapes)[j] = 0;
		delete shape;
	}

	delete mp_dynamicsWorld;
	delete mp_constraintSolver;
	delete mp_broadphase;
	delete mp_dispatcher;

	mp_collisionShapes->clear();

}

void Game::AddCollisionShape(btCollisionShape* collShape)
{
	assert(collShape);
	mp_collisionShapes->push_back(collShape);

}

